#include "wall_localization/wall_localizer.hpp"
#include <tf2/LinearMath/Quaternion.h>
#include <cmath>

namespace Wall_Localization
{
    WallLocalizer::WallLocalizer() : Node("wall_localizer") {
        
        // 声明参数
        warehouse_width_ = this->declare_parameter("warehouse.width", 16.0);        // x轴
        warehouse_height_ = this->declare_parameter("warehouse.height", 12.0);      // y轴

        laser_offset_x_ = this->declare_parameter("laser_offset_x", 12.0);  
        laser_offset_y_ = this->declare_parameter("laser_offset_y", 12.0);  

        ransac_distance_threshold_ = this->declare_parameter("ransac_distance_threshold", 0.02);
        ransac_max_iterations_ = this->declare_parameter("ransac_max_iterations", 200);

        /* laser_offset_z_ = this->declare_parameter("laser_offset_z", 0.0);  
        laser_roll_ = this->declare_parameter("laser_roll", 0.0);  
        laser_pitch_ = this->declare_parameter("laser_pitch", 0.0);  
        laser_yaw_ = this->declare_parameter("laser_yaw", 0.0);   */

        // 初始化机器人位姿
        current_pose_ = {0.0, 0.0, 0.0};
        pose_valid_ = false;
        latest_scan_ = nullptr;

        // 创建订阅和发布
        laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", 10, std::bind(&WallLocalizer::laserScanCallback, this, std::placeholders::_1));
        
        // 50ms/20Hz
        pose_timer_ = this->create_wall_timer(std::chrono::milliseconds(50), 
                                            std::bind(&WallLocalizer::poseTimerCallback, this));

        pose_json_pub_ = this->create_publisher<std_msgs::msg::String>("/fangshe/robot/status/pose", 10);
        pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("robot_pose", 10);

        RCLCPP_INFO(this->get_logger(), "Wall Localizer initialized with 20Hz pose publishing");
    }

    //////////////////////////////////////////////////////////////////////////////////////////
    //
    //
    //
    //            如果单线激光雷达位置更改，请更改yaml文件和segmentWalls角度分割数据
    //
    //
    //
    /////////////////////////////////////////////////////////////////////////////////////////

    // 回调函数
    void WallLocalizer::laserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {

        if(msg->ranges.empty()) {
            RCLCPP_WARN(this->get_logger(), "Empty laser scan data received!");
            return;
        }

        RCLCPP_INFO(this->get_logger(), "Received laser scan: %zu points", msg->ranges.size());

        // 保存最新的扫描数据
        latest_scan_ = msg;

        // 转换激光扫描数据为点云
        auto points = convertLaserScanToBaseLinkPoints(msg);

        // 检测墙壁
        auto Walls = detectWalls(points);

        // 在日志中输出前后左右墙壁信息
        //printWallInfo(Walls);

        // 定位
        estimatePoseFromWalls(Walls);

        
        if(pose_valid_)
        {
            std::lock_guard<std::mutex> lock(pose_mutex_);

            // 发布当前位姿
            latest_pose_.header.stamp = this->now();
            latest_pose_.header.frame_id = "map";

            // 设置坐标
            latest_pose_.pose.pose.position.x = current_pose_.x;
            latest_pose_.pose.pose.position.y = current_pose_.y;
            latest_pose_.pose.pose.position.z = 0.0;

            // 设置方向
            tf2::Quaternion q;
            q.setRPY(0, 0, current_pose_.theta);
            latest_pose_.pose.pose.orientation.x = q.x();
            latest_pose_.pose.pose.orientation.y = q.y();
            latest_pose_.pose.pose.orientation.z = q.z();
            latest_pose_.pose.pose.orientation.w = q.w();

            // 设置协方差
            for(int i = 0; i < 36; i++) {
                latest_pose_.pose.covariance[i] = (i % 7 == 0) ? 0.1 : 0.0;
            }

        }
    }


    // 将单线雷达的极坐标数据Rho、theta转换为包含x、y、z、intensity的点云数据
    // 将单线激光雷达坐标系的点云转换到机器人本体坐标系下
    std::vector<Eigen::Vector2d> WallLocalizer::convertLaserScanToBaseLinkPoints(
        const sensor_msgs::msg::LaserScan::SharedPtr msg){

        std::vector<Eigen::Vector2d> points_in_base_link;

        for(size_t i = 0; i < msg->ranges.size(); ++i) 
        {
            float range = msg->ranges[i];

            if(range > msg->range_min && range < msg->range_max) 
            {
                double angle = msg->angle_min + i * msg->angle_increment;

                double x_laser = range * std::cos(angle);
                double y_laser = range * std::sin(angle);

                // 将单线激光雷达坐标系的点云转换到机器人本体坐标系下
                double x_base_link = x_laser + laser_offset_x_;
                double y_base_link = y_laser + laser_offset_y_;

                points_in_base_link.emplace_back(x_base_link, y_base_link);
            }
        }  

        return points_in_base_link;
    }



    // 规定机器人坐标系下对应扇形区域为前后左右
    std::vector<std::vector<Eigen::Vector2d>> WallLocalizer::segmentWalls(const
         std::vector<Eigen::Vector2d>& points)
    {
        //前后左右
        std::vector<std::vector<Eigen::Vector2d>> wall_segments(4);

        for(const auto& point : points) {
            double angle = std::atan2(point.y(), point.x());

            // 遮挡角度为120°--147.5°
            
            //根据角度分配到不同墙面，前0右1后2左3
             if (angle >= -M_PI/6 && angle < M_PI/6)
            {
                wall_segments[0].push_back(point);  // 前墙，±30°
            } 
            else if (angle >= -3*M_PI/4 && angle < -M_PI/4) 
            {
                wall_segments[1].push_back(point);  // 右墙，-135°~-45°
            } 
            else if ((angle >= - M_PI && angle < -3*M_PI/4) ||
                    (angle >= 2.574 && angle < M_PI))
            {
                wall_segments[2].push_back(point);  // 后墙，-180°~-135° 和 147.5°~180°
            } 
            else if (angle >= M_PI/3 && angle < 2*M_PI/3) 
            {
                wall_segments[3].push_back(point);  // 左墙，60°~120°
            }
        }

        return wall_segments;
    }


    // 使用RANSAC将点云拟合成直线
    Line WallLocalizer::fitLineRANSAC(const std::vector<Eigen::Vector2d>& points,
                                    double threshold, int max_iterations) {

        if(points.size() < 2) {
            return Line{0, 0, 0, false};
        }
        
        Line best_line{0, 0, 0, false};
        int best_inliers = 0;

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_int_distribution<> dis(0, points.size() - 1);

        for (int i = 0; i < max_iterations; ++i) {
            // 随机选择两个点
            int idx1 = dis(gen);
            int idx2 = dis(gen);
            if (idx1 == idx2) continue;
            
            // 拟合直线
            Line line = fitLineTwoPoints(points[idx1], points[idx2]);
            
            // 计算内点数量（与直线距离小于阈值的点）
            int inliers = countInliers(points, line, threshold);
            
            //选择内点最多的直线作为最佳拟合
            if (inliers > best_inliers && inliers >= 5) {  // 至少5个内点
                best_inliers = inliers;
                best_line = line;
                best_line.valid = true;
            }
        }
        return best_line;
    }

    // ax+by+c=0 由两点计算组成的直线
    Line WallLocalizer::fitLineTwoPoints(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2) {
        double a = p2.y() - p1.y();
        double b = p1.x() - p2.x();
        double c = p2.x() * p1.y() - p1.x() * p2.y();
        return Line{a, b, c, true};
    }

    // 筛选内点，距离在阈值内的点即为内点
    int WallLocalizer::countInliers(const std::vector<Eigen::Vector2d>& points,
                                    const Line& line, double threshold) {
        int count = 0;
        for(const auto& point : points) {
            double distance = pointToLineDistance(point, line);
            if(distance < threshold) {
                count++;
            }
        }
        return count;
    }

    // 计算所有点到该条拟合直线的距离
    double WallLocalizer::pointToLineDistance(const Eigen::Vector2d& point, const Line& line) {
        if(line.a == 0 && line.b == 0) return 0;
        return std::abs(line.a * point.x() + line.b * point.y() + line.c) /
               std::sqrt(line.a * line.a + line.b * line.b);
    }

    Eigen::Vector2d WallLocalizer::calculateNormal(const Line& line) {
        double length = std::sqrt(line.a * line.a + line.b * line.b);
        if(length == 0) return Eigen::Vector2d(1, 0);
        return Eigen::Vector2d(line.a / length, line.b / length);
    }

    Eigen::Vector2d WallLocalizer::calculateLineCenter(const std::vector<Eigen::Vector2d>& points) {
        if(points.empty()) return Eigen::Vector2d(0, 0);

        Eigen::Vector2d sum(0, 0);
        for(const auto& point : points) {
            sum += point;
        }
        return sum / static_cast<double>(points.size());
    }

    // 确保法向量始终指向机器人/墙内
    Eigen::Vector2d WallLocalizer::ensureInwardNormal(const Eigen::Vector2d& normal,
                                                    const Eigen::Vector2d& centroid, 
                                                    const std::vector<Eigen::Vector2d>& points) 
    {
        if(points.empty()) {
            return normal; // 如果没有点云数据，返回原始法向量
        }
        
        // 计算从墙面向点云中心指向机器人原点的向量
        Eigen::Vector2d to_center = Eigen::Vector2d(0.0, 0.0) - centroid;
        
        // 归一化
        if(to_center.norm() > 1e-5) {
            to_center.normalize();
        } 
        else 
        {
            return normal; // 如果中心点与墙面点重合，返回原始法向量
        }
        
        // 如果法向量与指向中心的向量夹角大于90度（点积为负）
        // 则翻转法向量方向
        if(normal.dot(to_center) < 0) {
            return -normal;
        }
        
        return normal;
    }


    // 分割、拟合、计算前后左右墙壁法向量、点云中心
    std::vector<WallInfo> WallLocalizer::detectWalls(const std::vector<Eigen::Vector2d>& points) {
        
        //1. 分割墙面点云
        auto wall_segments = segmentWalls(points);

        //2. 对每个墙面进行拟合
        std::vector<WallInfo> walls(4);
        for (int i = 0; i < 4; ++i) {
            // 至少10个点才拟合
            if (wall_segments[i].size() >= 5) {  
                Line line = fitLineRANSAC(wall_segments[i], ransac_distance_threshold_, ransac_max_iterations_); 
                if (line.valid) 
                {
                    // 计算机器人中心到墙壁线段的垂直距离
                    double distance = pointToLineDistance(Eigen::Vector2d(0, 0), line);
                    // 计算墙壁线段的法向量（方向随机）
                    Eigen::Vector2d normal = calculateNormal(line);
                    // 计算点云中心点
                    Eigen::Vector2d center = calculateLineCenter(wall_segments[i]); 
                    // 确保法向量指向墙内/机器人
                    normal = ensureInwardNormal(normal, center, wall_segments[i]);
                    // 将以上信息分配给对应的墙壁
                    walls[i] = WallInfo{line, distance, normal, center, 
                                    static_cast<int>(wall_segments[i].size()), true, wall_segments[i]};
                } 
                else {
                    walls[i] = WallInfo{{0, 0, 0, false}, 0, Eigen::Vector2d(0, 0), 
                                    Eigen::Vector2d(0, 0), 0, false};
                }
            } 
            else {
                walls[i] = WallInfo{{0, 0, 0, false}, 0, Eigen::Vector2d(0, 0), 
                                Eigen::Vector2d(0, 0), 0, false};
            }
        }
        
        return walls;
    }



    // 在日志中输出墙壁基本信息
    void WallLocalizer::printWallInfo(const std::vector<WallInfo>& walls) {
        const char* wall_names[] = {"Front", "Right", "Back", "Left"};
        
        for (size_t i = 0; i < walls.size() && i < 4; ++i) {
            const auto& wall = walls[i];
            if (wall.valid) {
                RCLCPP_INFO(this->get_logger(), 
                        "Wall %s: dist=%.2fm, pts=%d, line(%.2fx+%.2fy+%.2f=0)",
                        wall_names[i], wall.distance, wall.point_count,
                        wall.line.a, wall.line.b, wall.line.c);
            }
        }
    }


void WallLocalizer::estimatePoseFromWalls(const std::vector<WallInfo>& walls) {

        // 寻找左墙和后墙
        const WallInfo* left_wall = nullptr;
        const WallInfo* back_wall = nullptr;
        
        // 左墙的索引值通常为[3]
        if (walls.size() > 3 && walls[3].valid) {
            left_wall = &walls[3];
        }
        // 后墙通常为[2]
        if (walls.size() > 2 && walls[2].valid) {
            back_wall = &walls[2];
        }

        // 如果找到了左墙和后墙
        if (left_wall && back_wall) {
            current_pose_.x = left_wall->distance;  // 距离左墙 = X坐标
            current_pose_.y = back_wall->distance;  // 距离后墙 = Y坐标

            // 实时计算角度：基于两个墙面的法向量
            double theta = calculateRobotOrientationFromWalls(*left_wall, *back_wall);
            current_pose_.theta = theta;  
                        
            pose_valid_ = true;
            
            RCLCPP_INFO(this->get_logger(), "Pose estimated - X: %.2f, Y: %.2f, Theta: %.2f", 
                        current_pose_.x, current_pose_.y, current_pose_.theta);
        } 
        else {
            pose_valid_ = false;
            RCLCPP_WARN(this->get_logger(), "Failed to estimate pose from walls");
        }
    }
    

    // 实时检测并计算机器人当前朝向
    double WallLocalizer::calculateRobotOrientationFromWalls(const WallInfo& left_wall, 
                                                       const WallInfo& back_wall) {

        /////////////////////////////////////////////////////////////////////
        // 
        // 以下对于全局坐标系、机器人坐标系的定义均采用标准坐标系
        //
        // 当机器人的偏航角theta=0时，机器人的x轴正方向与全局y轴正方向对齐
        //
        // 因此，为更好地与detectWalls函数计算出的墙壁法向量进行比较计算得出偏航角
        // 下方代码中的理想法向量均为机器人坐标系下的表示方式
        //
        // 左墙理想法向量指向机器人坐标系y轴负方向(0, -1)、全局x轴正方向 (1, 0)
        // 后墙理想法向量指向机器人坐标系x轴正方向(1, 0)、全局y轴正方向 (0, 1)
        //
        ////////////////////////////////////////////////////////////////////

        // 机器人坐标系下左、后墙理想归一化后的法向量
        Eigen::Vector2d ideal_left_normal(0.0, -1.0);     
        Eigen::Vector2d ideal_back_normal(1.0, 0.0);    

        // 归一化后实际观测到的左后墙法向量
        Eigen::Vector2d observed_left = left_wall.normal.normalized();
        Eigen::Vector2d observed_back = back_wall.normal.normalized();


        // 计算每个法向量与理想法向量的夹角
        // 计算从理想法向量旋转到观测法向量所需的角度
        double left_angle = std::atan2(ideal_left_normal.x() * observed_left.y() - ideal_left_normal.y() * observed_left.x(),
                                    ideal_left_normal.dot(observed_left));
        
        double back_angle = std::atan2(ideal_back_normal.x() * observed_back.y() - ideal_back_normal.y() * observed_back.x(),
                                    ideal_back_normal.dot(observed_back));

        
        // 此处theta角表示理想法向量到观测法向量的旋转角度的负角度
        // 为缩小误差及稳定theta的变化，此处采用左、后墙的偏航角的均值
        double theta = - (left_angle + back_angle) / 2.0 + (M_PI / 2) ;


        // 角度归一化
        while(theta > M_PI) theta -= 2 * M_PI;
        while(theta < -M_PI) theta += 2 * M_PI;

        // 输出法向量日志
        /* RCLCPP_INFO(this->get_logger(), "Debug - Left Wall Normal: (%.3f, %.3f), Back Wall Normal: (%.3f, %.3f)",
                    left_wall.normal.x(), left_wall.normal.y(),
                    back_wall.normal.x(), back_wall.normal.y());
        RCLCPP_INFO(this->get_logger(), "Debug - Left Wall Normal (Norm): (%.3f, %.3f), Back Wall Normal (Norm): (%.3f, %.3f)",
                    observed_left.x(), observed_left.y(),
                    observed_back.x(), observed_back.y());
        RCLCPP_INFO(this->get_logger(), "Debug - Ideal Left: (%.3f, %.3f), Ideal Back: (%.3f, %.3f)",
                    ideal_left_normal.x(), ideal_left_normal.y(),
                    ideal_back_normal.x(), ideal_back_normal.y());
        RCLCPP_INFO(this->get_logger(), "Debug - Left Angle: %.3f, Back Angle: %.3f, Theta: %.3f",
                    left_angle, back_angle, theta); */

        return theta;


    }


    // 输出位姿
    void WallLocalizer::poseTimerCallback()
    {
        geometry_msgs::msg::PoseWithCovarianceStamped pose_to_publish;
        std_msgs::msg::String json_msg;

        {
            std::lock_guard<std::mutex> lock(pose_mutex_);
            pose_to_publish = latest_pose_;

            PoseJsonConverter json_pose;
            json_pose.set_from_pose(current_pose_.x, current_pose_.y, current_pose_.theta);   
            json_msg.data = json_pose.get_json_msg();
        }
        
        if(pose_valid_){
            // 发布话题
            pose_pub_->publish(pose_to_publish);

            // 发布json格式的位姿
            pose_json_pub_->publish(json_msg);


            RCLCPP_DEBUG(this->get_logger(), "Published pose at fixed rate: x=%.2f, y=%.2f, theta=%.2f", 
                        pose_to_publish.pose.pose.position.x,
                        pose_to_publish.pose.pose.position.y,
                        current_pose_.theta);
        }

    }

}

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<Wall_Localization::WallLocalizer>());
    rclcpp::shutdown();
    return 0;
}